function [CFM] = multvecattdet_svd(vM, vF, aOrWndLen, normalizeInput, bp) %#codegen
%MULTVECATTDET_SVD 多矢量定姿算法-svd方法
%
% Input Arguments:
% # vM: 3*n矩阵，M系下的一组参考矢量，n表示矢量数，单位无
% # vF: 3*n矩阵，F系下的一组参考矢量，n表示矢量数，单位无
% # aOrWndLen: 标量或1*n向量，标量表示滑动标准差窗口长度（以窗口滑动标准差倒数作为权重值），向量表示指定的权重值，默认全为1/n
% # normalizeInput: 逻辑标量，true表示使vM、vF向量归一化
% # bp: 向量，计算滑动标准差的分段点，按每段计算滑动标准差，默认为空
%
% Output Arguments:
% # CFM: 3*3矩阵，姿态矩阵，由F系到M系的转换矩阵，单位无
%
% References:
% #《应用导航算法工程基础》“多矢量定姿算法及其误差”

n = size(vM, 2);
if nargin < 5
    bp = [];
end
if nargin < 4
    normalizeInput = false;
end
if (nargin < 3) || isempty(aOrWndLen)
    aOrWndLen = 1/n * ones(1, n);
end

if normalizeInput
    vM = vM ./ vecnorm(vM, 2, 1);
    vF = vF ./ vecnorm(vF, 2, 1);
end
if isscalar(aOrWndLen) % 滑动标准差窗口长度
    wndLen = aOrWndLen;
    a = NaN(3, n);
    bp = [0; bp(:); n];
    for i=2:length(bp)
        a(:, (bp(i-1)+1):bp(i)) = movstd(vM(:, (bp(i-1)+1):bp(i)), wndLen, 0, 2);
    end
    a = 1 ./ vecnorm(a, 2, 1);
    a = a / sum(a);
else
    a = aOrWndLen;
end
B = zeros(3);
for i = 1:length(vM(1, :))
    B = B + a(i)*vM(:, i)*vF(:, i)';
end
[U, ~, V] = svd(B);
CFM = U*diag([1 1 det(U)*det(V)])*V'; % REF1 4.3.255式
end  